/****************************************************************
* Created by Jimmy on 2025/2/24.
****************************************************************/

#include "mpu6050.h"


#define MPU6050_USING_DMA
typedef enum
{
    STATE_INIT,
    STATE_READ_ACCEL,
    STATE_READ_GYRO,
    STATE_PROCESS_DATA,
    STATE_IDLE,

}MPU6050_READ_STATE_T;

static int16_t Mpu6050Addr = 0x69;
extern I2C_HandleTypeDef hi2c2;
extern DMA_HandleTypeDef hdma_i2c2_rx;
extern DMA_HandleTypeDef hdma_i2c2_tx;
static MPU6050DATATYPE  g_mpu6050_data;
uint8_t mpu6050ID;
static MPU6050_READ_STATE_T g_current_read_state= STATE_INIT;
uint8_t accelData[6];
uint8_t gyroData[6];
uint8_t i2c_read_flag;

void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *hi2c)
{

}

void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c)
{
  if(hi2c->Instance==I2C2)
  {
    i2c_read_flag=1;
    switch (g_current_read_state)
    {
      case STATE_READ_ACCEL:
        // 启动DMA读取加速度数据
        i2c_read_flag = 0;
        g_current_read_state = STATE_READ_GYRO;
        break;
      case STATE_READ_GYRO:
        i2c_read_flag = 0;
        g_current_read_state = STATE_PROCESS_DATA;
        break;
    }
  }

}
void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef *hi2c)
{
  if(hi2c->Instance==I2C2)
  {

  }
}
void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c)
{
  if(hi2c->Instance==I2C2)
  {

  }
}

int16_t Sensor_I2C2_Serch(void)
{
  for(uint8_t i = 1; i < 255; i++)
  {
    if(HAL_I2C_IsDeviceReady(&hi2c2, i, 1, 1000) == HAL_OK)
    {
      Mpu6050Addr = i>>1;
      return i>>1;
    }
  }
  return -1; // 搜索失败
}

void mpu6050_init()
{

  /*MPU6050寄存器初始化，需要对照MPU6050手册的寄存器描述配置，此处仅配置了部分重要的寄存器*/
  uint8_t data[6] = {0x01,0x00,0x09,0x06,0x18,0x18};
  // 唤醒MPU6050
//  HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDRESS, MPU6050_PWR_MGMT_1,
//                    1, &data, 1, HAL_MAX_DELAY);
  MPU6050_WriteReg(MPU6050_PWR_MGMT_1,data,1);      //电源管理寄存器1，取消睡眠模式，选择时钟源为X轴陀螺仪
  MPU6050_WriteReg(MPU6050_PWR_MGMT_2,&data[1],1);  //电源管理寄存器2，保持默认值0，所有轴均不待机
  MPU6050_WriteReg(MPU6050_SMPLRT_DIV,&data[2],1);  //采样率分频寄存器，配置采样率
  MPU6050_WriteReg(MPU6050_CONFIG,&data[3],1);      //配置寄存器，配置DLPF
  MPU6050_WriteReg(MPU6050_GYRO_CONFIG,&data[4],1); //陀螺仪配置寄存器，选择满量程为±2000°/s
  MPU6050_WriteReg(MPU6050_ACCEL_CONFIG,&data[5],1);//加速度计配置寄存器，选择满量程为±16g

}

// 使用DMA读取MPU6050数据
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t* Data,uint16_t length)
{
  // 发送要读取的寄存器地址
#ifndef MPU6050_USING_DMA
  HAL_StatusTypeDef status = HAL_I2C_Mem_Write(&hi2c2, MPU6050_ADDRESS, RegAddress,
                    I2C_MEMADD_SIZE_8BIT, Data, length,HAL_MAX_DELAY);

#else
  HAL_StatusTypeDef status =HAL_I2C_Mem_Write_DMA(&hi2c2, MPU6050_ADDRESS, RegAddress,
                                                  I2C_MEMADD_SIZE_8BIT, Data,length);


#endif
  if (status != HAL_OK) {
    // 处理错误
  }
  while(HAL_I2C_GetState(&hi2c2) != HAL_I2C_STATE_READY){
  }

}

void MPU6050_ReadReg(uint8_t RegAddress, uint8_t* Data,uint16_t length)
{
#ifndef MPU6050_USING_DMA
  HAL_StatusTypeDef status = HAL_I2C_Mem_Read(&hi2c2,MPU6050_ADDRESS,RegAddress,
                                             I2C_MEMADD_SIZE_8BIT,Data,length,HAL_MAX_DELAY);
#else
  HAL_StatusTypeDef status = HAL_I2C_Mem_Read_DMA(&hi2c2,MPU6050_ADDRESS,RegAddress,
                                                  I2C_MEMADD_SIZE_8BIT,Data,length);
#endif
  if (status != HAL_OK) {
    // 处理错误
  }

  while (HAL_I2C_GetState(&hi2c2) != HAL_I2C_STATE_READY) {
  }
}


#ifndef MPU6050_USING_DMA
/**
  * 函    数：MPU6050获取数据
  * 参    数：AccX AccY AccZ 加速度计X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 参    数：GyroX GyroY GyroZ 陀螺仪X、Y、Z轴的数据，使用输出参数的形式返回，范围：-32768~32767
  * 返 回 值：无
  */
MPU6050DATATYPE MPU6050_GetData()
{

  MPU6050DATATYPE sensorDat={0};
  uint8_t DataH, DataL;								    //定义数据高8位和低8位的变量

  MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H,&DataH,1);		//读取加速度计X轴的高8位数据
  MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L,&DataL,1);		//读取加速度计X轴的低8位数据
  sensorDat.Accel_X = (DataH << 8) | DataL;

  MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H,&DataH,1);		//读取加速度计Y轴的高8位数据
  MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L,&DataL,1);		//读取加速度计Y轴的低8位数据
  sensorDat.Accel_Y = (DataH << 8) | DataL;

  MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H,&DataH,1);		//读取加速度计Z轴的高8位数据
  MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L,&DataL,1);		//读取加速度计Z轴的低8位数据
  sensorDat.Accel_Z = (DataH << 8) | DataL;

  MPU6050_ReadReg(MPU6050_GYRO_XOUT_H,&DataH,1);		//读取陀螺仪X轴的高8位数据
  MPU6050_ReadReg(MPU6050_GYRO_XOUT_L,&DataL,1);		//读取陀螺仪X轴的低8位数据
  sensorDat.Gyro_X = (DataH << 8) | DataL;

  MPU6050_ReadReg(MPU6050_GYRO_YOUT_H,&DataH,1);		//读取陀螺仪Y轴的高8位数据
  MPU6050_ReadReg(MPU6050_GYRO_YOUT_L,&DataL,1);		//读取陀螺仪Y轴的低8位数据
  sensorDat.Gyro_Y = (DataH << 8) | DataL;

  MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H,&DataH,1);		//读取陀螺仪Z轴的高8位数据
  MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L,&DataL,1);		//读取陀螺仪Z轴的低8位数据
  sensorDat.Gyro_Z = (DataH << 8) | DataL;

  return sensorDat;
}
#else

MPU6050DATATYPE MPU6050_GetData()
{
  return g_mpu6050_data;
}

void MPU6050_acquire()
{
  switch (g_current_read_state)
  {
    case STATE_INIT:
      // 初始化完成，进入读取加速度数据状态
      g_current_read_state = STATE_READ_ACCEL;
      break;
    case STATE_READ_ACCEL:
      // 启动DMA读取加速度数据
      MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H,accelData,6);
      break;
    case STATE_READ_GYRO:
      // 等待加速度数据读取完成，启动DMA读取角速度数据
      MPU6050_ReadReg(MPU6050_GYRO_XOUT_H,gyroData,6);
      break;
    case STATE_PROCESS_DATA:

        g_mpu6050_data.Accel_X = (accelData[0] << 8) | accelData[1];
        g_mpu6050_data.Accel_Y = (accelData[2] << 8) | accelData[3];
        g_mpu6050_data.Accel_Z = (accelData[4] << 8) | accelData[5];
        g_mpu6050_data.Gyro_X = (gyroData[0] << 8) | gyroData[1];
        g_mpu6050_data.Gyro_Y = (gyroData[2] << 8) | gyroData[3];
        g_mpu6050_data.Gyro_Z = (gyroData[4] << 8) | gyroData[5];
        g_current_read_state = STATE_IDLE;

      break;
    case STATE_IDLE:
      g_current_read_state = STATE_READ_ACCEL;
      break;
    default:
      g_current_read_state = STATE_INIT;
      break;
  }
}

#endif

uint8_t MPU6050_GetID()
{

  MPU6050_ReadReg(MPU6050_WHO_AM_I,&mpu6050ID,1);
  return mpu6050ID;
}